#include "Chassis.h"
#include "../Robotmap.h"
#include "../Commands/DriveWithJoysticksMec.h"
#include "../Commands/DriveBackAndForth.h"


Chassis::Chassis() : Subsystem("Chassis") {
	drive = new RobotDrive(LEFTFRONTMOTOR,   LEFTBACKMOTOR, RIGHTFRONTMOTOR, RIGHTBACKMOTOR);
	drive->SetSafetyEnabled(false);
}

void Chassis::InitDefaultCommand() {
	SetDefaultCommand(new DriveWithJoysticksMec());
}

void Chassis::goStraight(double speed = 1.0) {
	double crouch = CommandBase::oi->getCrouch();
	drive->MecanumDrive_Cartesian(speed*crouch, 0, 0);
}

void Chassis::goBack(double speed = 1.0){
	double crouch = CommandBase::oi->getCrouch();	
	drive->MecanumDrive_Cartesian(-speed*crouch, 0, 0);
}

void Chassis::turnLeft() {
	drive->MecanumDrive_Cartesian(0, 0, 1);
}

void Chassis::turnRight() {
	drive->MecanumDrive_Cartesian(0, 0, -1);
}



void Chassis::driveWithJoysticksMec(float xvalue, float yvalue , float spinvalue) {
//	cout << "xvalue = " << xvalue << ", yvalue = " << yvalue << ", spinvalue = " << spinvalue << endl;
	drive->MecanumDrive_Cartesian(xvalue, yvalue, spinvalue);
}
